/**********************************************
*  Namespace: AI
*  Classes:   SteeringArrive
*  Desc:      Updates positions using a Arrive
*             algorithm in 2d or 3d cases
*  Author:    Vaios Kalpias-Ilias (C) 2008-2009
***********************************************/

#include "SteeringArrive.h"

namespace AI
{
	//---------------------------------------------
	/**
	* Ctor
	*/
	SteeringArrive::SteeringArrive()
	{
	}
	//---------------------------------------------
	/**
	* Dtor
	*/
	SteeringArrive::~SteeringArrive()
	{
	}
	//---------------------------------------------
	/**
	* Returns new kinematic data in 2d
	* @param current
	* @param target
	*/
	Steering2d SteeringArrive::GetSteering(const Kinematic2d& current,
									   const Kinematic2d& target)
	{
		mth::Vector2 direction(target.position - current.position);
		float   distance = direction.Length();

		// check if we are there and return no steering
		Steering2d steering;
		if (distance < targetRadius)
		{
			return steering;
		}

		// if we are outside the slow radius travel at max speed
		float targetSpeed;
		if (distance > slowRadius)
		{
			targetSpeed = maxSpeed;
		}
		// otherwise calculate a scaled speed
		else
		{
			targetSpeed = maxSpeed * distance / slowRadius;
		}

		// targetVelocity cobines speed and direction
		mth::Vector2 targetVelocity(direction.Normalized());
		targetVelocity *= targetSpeed;

		// acceleration tries to get to the target velocity
		steering.linear = targetVelocity - current.velocity;
		steering.linear /= timeToTarget;

		// trim acceleration
		if (steering.linear.Length() > maxAcceleration)
		{
			steering.linear.Normalize();
			steering.linear *= maxAcceleration;
		}

		return steering;
	}

}
